/*
 * Servo.c
 *
 * Created: 3/21/2013 6:50:14 AM
 *  Author: Parag
 */ 

#include "Servo.h"

void ConfigRightServo(enum Angles ang)
{
	switch(ang)
		{
			case North:
					OCR1A = 310;
				break;
			case NW:
					OCR1A = 255;
				break;
			case WNW:
					OCR1A = 228;
				break;
			case West:
					OCR1A = 200;
				break;
			case WSW:
					OCR1A = 173;
				break;
			case SW:
					OCR1A = 145;
				break;
			case South:
					OCR1A = 90;
				break;
			default:
				OCR1A = 200;		
		}
}
void ConfigLeftServo(enum Angles ang)
{
	switch(ang)
	{
		case North:
				OCR1B = 90;
			break;
		case NW:
				OCR1B = 145;
			break;
		case WNW:
				OCR1B = 173;
			break;
		case West:
				OCR1B = 200;
			break;
		case WSW:
				OCR1B = 228;
			break;
		case SW:
				OCR1B = 255;
			break;
		case South:
				OCR1B = 310;
			break;
		default:
				OCR1B = 200;		
	}
}

//	ASSUMING: using the same servo model for Rudder; IMPORTANT assumption since different servo models have difference in Duty Cycle Range
//	ASSUMING: the rudder is orienting similar to right servo to calculate all the angles	
void ConfigRudderServo(enum Angles ang)
{
	switch(ang)
	{
		case North:
				OCR0A = 32;
			break;
		case NW:
				OCR0A = 26;
			break;
		case WNW:
				OCR0A = 23;
			break;
		case West:
				OCR0A = 20;
			break;
		case WSW:
				OCR0A = 18;
			break;
		case SW:
				OCR0A = 15;
			break;
		case South:
				OCR0A = 9;
			break;
		default:
				OCR0A = 20;
	}
}

void ServoInit()
{
	//Configure TIMER1
	TCCR1A|=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11);			//NON Inverted PWM
	TCCR1B|=(1<<WGM13)|(1<<WGM12) |(1<<CS11)|(1<<CS10);	//PRESCALER=64 MODE 14(FAST PWM)

	ICR1=2499;											//Setting Top; fPWM=50Hz (Period = 20ms Standard)
	DDRB|= (1<<PB5)|(1<<PB6);							//PB5 = right fin servo 
														//PB6 = left fin servo
	
	
	//Configure Timer0 for rudder
	//setup the timer
	TCCR0A = (1<<WGM01) | (1<<WGM00);						 //set pwm, TOP = FF, Fast PWM mode
	TCCR0A = (1<<COM0A1) | (0<<COM0A0);						//non inverted mode
	
	TCCR0A = (1<<CS02) | (0<<CS01) | (0<<CS00);				//Prescale = 256; fPWM  =  122.07Hz as recommended by Dr. R
															// or else i can set it to 55Hz but 50 Hz can never be achieved with an 8bit
															//timer since the Top is hard set to 255
															

	
	DDRB |= (1<<PB4);										//PB4 = Rudder fin;

	sei();
	
	//Initialize Servos
	ConfigLeftServo(West);
	ConfigRightServo(West);
	ConfigRudderServo(West);
	_delay_ms(1000);									// allow the servos to initialize
}